/**
 * @author Morten Tranberg Hansen (mth@daimi.au.dk)
 * @date   August 28 2008
 */

//#include "Print.h"
#include "EnergyMeasurement.h"

#ifndef PLATFORM_TELOSB
#error "Telosb platform specific code compiled."
#endif
//#warning "Compiling msp430 specific code."

module EnergyMeasurementP {

	uses {		
		interface Boot;
		interface State;
		interface BusyWait<T32khz, uint16_t> as Wait;
		interface Counter<T32khz, uint32_t> as Counter;
		interface Leds;
		
		interface StdControl as UartControl;
		interface UartStream;
		interface Crc;
				
		interface GpioInterrupt as ShiftInterrupt; // shift on falling edge		
		
		interface GeneralIO as Shift; 	// shift on falling edge
		interface GeneralIO as Shift2; 	// So we can set it as input.
		interface GeneralIO as Charge1;
		interface GeneralIO as Charge2;
		interface GeneralIO as Use1;
		interface GeneralIO as Use2;
		
		interface GeneralIO as ADC6;
		interface GeneralIO as ADC7;
		interface GeneralIO as ADC3;
		interface GeneralIO as I2C_SCL;
		interface GeneralIO as I2C_SDA;
		interface GeneralIO as GIO0;
		
		interface GeneralIO as UsbControl;
	}

} implementation {

	enum {
		S_ONE = 1,
		S_TWO = 2,
	};

	uint32_t startTime;			// start time of a run
	uint32_t seqno;				// current sequence number doing a run
	energy_data_msg_t data;		// message buffer
	norace bool shiftInterrupt = FALSE;// controls of the waiting loop should wait for a shift rise

	task void wait();
	void start();
	void stop();	
	void sendData();
	void shift_to_one();
	void shift_to_two();
	
	void init() {		
		call Shift.makeInput();
		call Shift2.makeInput();
		call Charge1.makeOutput();
		call Charge2.makeOutput();
		call Use1.makeOutput();
		call Use2.makeOutput();
		
		call ADC6.makeInput();
		call ADC7.makeInput();
		call ADC3.makeInput();
		call I2C_SCL.makeInput();
		call I2C_SDA.makeInput();
		call GIO0.makeInput();
		
		call UsbControl.makeOutput();
		
		// set l2h interrupt on GIO1
		call ShiftInterrupt.enableRisingEdge();

		stop();
	}


	event void Boot.booted() {
		init();
		if(call UartControl.start()!=SUCCESS || call UartStream.enableReceiveInterrupt()!=SUCCESS) {
			call Leds.led0On(); call Leds.led1On(); call Leds.led2On();
		}
	}
	
	event async void UartStream.receivedByte(uint8_t byte){
		switch(byte) {
		case ENERGY_ACTION_START:
			if(call State.isIdle()) {
				start();
			} else {
				call Leds.led0Toggle();
			}
			break;
		case ENERGY_ACTION_STOP:
			if(!call State.isIdle()) {
				stop();
			} else {
				call Leds.led0Toggle();
			}
			break;
		}
	}

	event async void UartStream.receiveDone(uint8_t *buf, uint16_t len, error_t error){}
	
	event async void UartStream.sendDone(uint8_t *buf, uint16_t len, error_t error){
		if(error!=SUCCESS) {
			call Leds.led0Toggle();
		}
	}
	
	task void wait() {
		bool rised = FALSE;			
		while(!call State.isIdle()) {
			// Wait till Shift rises (check Shift interrupt flag))
			if(!rised && !shiftInterrupt) {
				continue;
			}
			shiftInterrupt = FALSE;
			rised = TRUE;
			
			// Check for falling edge 
			if(!call Shift.get()) {
				// Clear Shift interrupt flag 
				rised = FALSE;
				
				// Wait for the capacitors to get charged.	
				switch(call State.getState()) {
					case S_ONE:
						shift_to_two();
						sendData();
						break;
					case S_TWO:		
						shift_to_one();
						sendData();
						break;
				}
			}
		}
	}
	
	void start() {
		call UsbControl.clr();
		
		atomic {
			call Use1.clr();
			call Use2.clr();		
			call Charge1.set();
			call Charge2.set();
		
			// Wait for the capacitors to get charged.
			while(!call Shift.get());
			//call Wait.wait(WAIT_CHARGE);

			call Charge1.clr();
			// Clear Shift interrupt flag
			atomic shiftInterrupt = FALSE;
			call Use1.set();
			startTime = call Counter.get();
			seqno = 0;
			call State.forceState(S_ONE);
		}
		
		//print("Startup complete");
		call Leds.led1On();
		post wait();
	}
	
	void stop() {
		atomic {	
			call Charge1.clr();
			call Charge2.clr();
			call Use1.clr();
			call Use2.clr();
			call State.toIdle();
		}
		
		call Leds.led1Off();
		call UsbControl.set();
		//print("Cleanup complete");
	}
	
	void sendData() {

		atomic data.time = call Counter.get() - startTime;
		atomic data.seqno = seqno++;

		atomic {
			#define PORT6IN (*TCAST(volatile TYPE_PORT_IN* ONE, P6IN_))
			//data.flags = (PORT6IN & (0x01 << 6)) << 0 | (PORT6IN & (0x01 << 7)) << 1 | (PORT6IN & (0x01 << 3)) << 2;  
			
			data.flags = 0;
			if(call ADC6.get()) data.flags += (1 << 0);
			if(call ADC7.get()) data.flags += (1 << 1);
			if(call ADC3.get()) data.flags += (1 << 2);
			//if(call State3.get() && !(P2IFG & (1 << 0))) data.flags += (1<<2); 
			P2IFG &= ~(1 << 0);
		}
	
		if(call UartStream.send(TCAST(uint8_t*, &data), sizeof(energy_data_msg_t))!=SUCCESS) {
			//print("ERROR: Could not send to serial.");
			call Leds.led0Toggle();
		} else {
			call Leds.led2Toggle();
		}
	}

	void shift_to_one() {
		atomic {
			call Charge1.clr();
			//call Wait.wait(WAIT_BETWEEN_PORTS);
			call Use1.set();
			//call Wait.wait(WAIT_BETWEEN_PORTS);
			call Use2.clr();
			//call Wait.wait(WAIT_BETWEEN_PORTS);
			call Charge2.set();
			call State.forceState(S_ONE);
		}
	}
	
	void shift_to_two() {
		atomic {
			call Charge2.clr();
			//call Wait.wait(WAIT_BETWEEN_PORTS);
			call Use2.set();
			//call Wait.wait(WAIT_BETWEEN_PORTS);
			call Use1.clr();
			//call Wait.wait(WAIT_BETWEEN_PORTS);
			call Charge1.set();
			call State.forceState(S_TWO);
		}
	}


	async event void Counter.overflow(){
		stop();
		call Leds.led0On();
		call Leds.led1On();
		call Leds.led2On();
	}

	async event void ShiftInterrupt.fired(){
		atomic shiftInterrupt = TRUE;
	}
}
